#include "distancefilter.h"
#include <iostream>
#include <stdio.h>
#include <string.h>

AI_SOKOBAN::Distancefilter::Distancefilter(const Level& level){

  distMatrix = new int[level.getCols()*level.getRows()];
  //std::cout<<level.getCols()<< " " <<level.getRows()<<std::endl;

  memset(distMatrix,-1,(level.getCols()*level.getRows()*sizeof(int)));

  calcGoalDistanceMatrix(level, distMatrix);
  /*
  for(int i = 0; i < level.getCols()*level.getRows(); i++){
	  std::cout<<distMatrix[i]<< " ";
	  if((i+1) % level.getCols() == 0) std::cout<<std::endl;
  }*/
}

double AI_SOKOBAN::Distancefilter::evalLevel(const Level& level, const Move& move)
{
	int current_x = move.getX();
	int current_y = move.getY();
	Direction dir = move.getDir();

	int rows = level.getRows();
	int cols = level.getCols();

	int next_x = current_x;
	int next_y = current_y;

    if(dir == UP){
	  current_y--;
      next_y = next_y - 2;
    }
    else if(dir == DOWN){
	  current_y++;
      next_y = next_y + 2;
    }
    else if(dir == RIGHT){
	  current_x++;
      next_x = next_x + 2;
    }
    else if(dir == LEFT){
	  current_x--;
      next_x = next_x - 2;
    }
    else
      std::cout << "error: no direction!" << std::endl;
	
	if(distMatrix[current_x + (cols * current_y)] > distMatrix[next_x + (cols * next_y)]) return 1.5;
	else return 0.5;
}

int AI_SOKOBAN::Distancefilter::minNrOfMoves(const Level& level){

	int rows = level.getRows();
	int cols = level.getCols();

	int nrOfMoves = 0;

	for(int i = 0; i < cols; i++){
		for(int j = 0; j < rows; j++){
			if(level.at(i,j) == '$') nrOfMoves = nrOfMoves + distMatrix[i + (cols * j)];
		}
	}
	return nrOfMoves;
}


void AI_SOKOBAN::Distancefilter::calcGoalDistanceMatrix(const Level& level, int * distMatrix){

	int rows = level.getRows();
	int cols = level.getCols();

	bool* visited = new bool[cols*rows];

	for(int i = 0; i < cols; i++){
		for(int j = 0; j < rows; j++){
			if(level.at(i,j) != '#'){
				memset(visited,false,cols*rows);
				std::queue<triplet> q;
				triplet start;
				start.x = i;
				start.y = j;
				start.dist = 0;
				q.push(start);
				visited[start.x + (cols * start.y)] = true;

				while(!q.empty()){
					triplet current = q.front();
					q.pop();

					if(level.at(current.x,current.y) == '.' || level.at(current.x,current.y) == '*'){
						distMatrix[i + (cols * j)] = current.dist;	
						break;
					}

					for(int n = 0; n < 4; n++){
						triplet w;
						if(n == 0){
							w.x = current.x + 1;
							w.y = current.y;
						}
						if(n == 1){
							w.x = current.x - 1;
							w.y = current.y;
						}
						if(n == 2){
							w.x = current.x;
							w.y = current.y + 1;
						}
						if(n == 3){
							w.x = current.x;
							w.y = current.y - 1;
						}

						w.dist = current.dist + 1;

						if(level.at(w.x,w.y) != '#' && visited[w.x + (cols * w.y)] == false && w.x < cols && w.y < rows && w.x >= 0 && w.y >= 0){
							q.push(w);
							visited[w.x + (cols * w.y)] = true;
						}
					}
				}
				
			}
		}
	}
	delete[] visited;
	visited = NULL;
}